
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_wpnav.c
  * @author     baiyang
  * @date       2022-3-10
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_wpnav.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/
// maximum velocities and accelerations
#define WPNAV_ACCELERATION              250.0f      // maximum horizontal acceleration in cm/s/s that wp navigation will request
#define WPNAV_WP_SPEED                 1000.0f      // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN               20.0f      // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_RADIUS                 200.0f      // default waypoint radius in cm
#define WPNAV_WP_RADIUS_MIN               5.0f      // minimum waypoint radius in cm
#define WPNAV_WP_SPEED_UP               250.0f      // default maximum climb velocity
#define WPNAV_WP_SPEED_DOWN             150.0f      // default maximum descent velocity
#define WPNAV_WP_ACCEL_Z_DEFAULT        100.0f      // default vertical acceleration between waypoints in cm/s/s
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void assign_param(mc_wpnav* wpnav);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   wpnav  
  * @param[in]   ahrs  
  * @param[in]   pos_control  
  * @param[out]  
  * @retval      
  * @note        
  */
void wpnav_ctor(mc_wpnav* wpnav, ahrs_view* ahrs, Position_ctrl* pos_control)
{
    wpnav->_ahrs             = ahrs;
    wpnav->_pos_control      = pos_control;

    wpnav->_rangefinder_healthy = false;

    assign_param(wpnav);
}

// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum WPNavTerrainSource wpnav_get_terrain_source(mc_wpnav* wpnav)
{
    // use range finder if connected
    if (wpnav->_rangefinder_available && wpnav->_rangefinder_use) {
        return WPNAV_TERRAIN_FROM_RANGEFINDER;
    }

    return WPNAV_TERRAIN_UNAVAILABLE;
}

// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool wpnav_get_terrain_offset(mc_wpnav* wpnav, float* offset_cm)
{
    // calculate offset based on source (rangefinder or terrain database)
    switch (wpnav_get_terrain_source(wpnav)) {
    case WPNAV_TERRAIN_UNAVAILABLE:
        return false;
    case WPNAV_TERRAIN_FROM_RANGEFINDER:
        if (wpnav->_rangefinder_healthy) {
            *offset_cm = wpnav->_ahrs->relpos_cm.z - wpnav->_rangefinder_alt_cm;
            return true;
        }
        return false;
    case WPNAV_TERRAIN_FROM_TERRAINDATABASE: 
        return false;
    }

    // we should never get here
    return false;
}

// convert location to vector from ekf origin.  terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
//      returns false if conversion failed (likely because terrain data was not available)
bool wpnav_get_vector_NEU(const Location *loc, Vector3f_t *vec, bool *terrain_alt)
{
    // convert location to NE vector2f
    Vector2f_t res_vec;
    if (!location_get_vector_xy_from_origin_ne(loc, &res_vec)) {
        return false;
    }

    // convert altitude
    if (location_get_alt_frame(loc) == ALT_FRAME_ABOVE_TERRAIN) {
        int32_t terr_alt;
        if (!location_get_alt_cm(loc, ALT_FRAME_ABOVE_TERRAIN, &terr_alt)) {
            return false;
        }
        vec->z = terr_alt;
        *terrain_alt = true;
    } else {
        *terrain_alt = false;
        int32_t temp_alt;
        if (!location_get_alt_cm(loc, ALT_FRAME_ABOVE_ORIGIN, &temp_alt)) {
            return false;
        }
        vec->z = temp_alt;
        *terrain_alt = false;
    }

    // copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful
    vec->x = res_vec.x;
    vec->y = res_vec.y;

    return true;
}

/**
  * @brief       
  * @param[in]   wpnav  
  * @param[out]  
  * @retval      
  * @note        
  */
static void assign_param(mc_wpnav* wpnav)
{
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED), &wpnav->_wp_speed_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_RADIUS), &wpnav->_wp_radius_cm);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED_UP), &wpnav->_wp_speed_up_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_SPEED_DN), &wpnav->_wp_speed_down_cms);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_ACCEL), &wpnav->_wp_accel_cmss);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_ACCEL_Z), &wpnav->_wp_accel_z_cmss);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_RFND_USE), &wpnav->_rangefinder_use);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_JERK), &wpnav->_wp_jerk);
    param_link_variable(PARAM_ID(WPNAV, WPNAV_TER_MARGIN), &wpnav->_terrain_margin);
}

/*------------------------------------test------------------------------------*/


